CON
        _clkmode = xtal1 + pll16x                                               
        _xinfreq = 5_000_000

  ServoCh1 = 16
  ServoCh2 = 17
  ServoCh3 = 18
  ServoCh4 = 19

  Forward  = 1
  Reverse  = 2
  Left     = 3
  Right    = 4
  Stop     = 5

VAR
  long stack[9]
  byte arm_servo
  byte clamp_servo
  
OBJ
  pst : "Parallax Serial Terminal"
  PS2 : "PS2_Controller"
  Num : "Simple_Numbers"
  servo : "Servo32v7"
  
PUB Main
  servo.start
  pst.start(115200)
  pst.str(String("Beginning program for testing the PS2 controller...",13))

  
  {Angular Servo Test}
  arm_servo := 100
  clamp_servo := 90
  
  coginit(1,MoveMotor(20,arm_servo),@stack)
  coginit(2,MoveMotor(21,90),@stack+100)
  waitcnt(clkfreq + cnt)

  {Continous Servo Test}
  coginit(4, Drive(Stop),@stack)
  waitcnt(clkfreq + cnt)


  
  ps2.start(12, 5000)
  repeat
    PS2_Interpret 

PUB PS2_Interpret
    waitcnt(clkfreq/2000 + cnt)                                                 'the higher the number, the faster the process is evaluated
    {Buttons}
    if (PS2.get_Data1 == $FFF75A73)
      pst.str(String("Pressed: START",13))
      
      
    if (PS2.get_Data1 == $FFFE5A73)
      pst.str(String("Pressed: SELECT",13))
      

    if (PS2.get_Data1 == $BFFF5A73)                                             'X Button
      pst.str(String("Pressed the X Button",13))
      
      
    if (PS2.get_data1 == $DFFF5A73)                                             'O Button
      pst.str(String("Pressed the Y Button",13))}
 
    {Right Control Stick}
    if (PS2.get_RightY <> 0) &(PS2.get_RightY <> 255) & (PS2.get_RightX <> 0) & (PS2.get_RightX <> 255)
      pst.str(String("Right Control Stick Not Moving",13))

    CASE PS2.get_RightY
      0: 
        pst.str(String("Right Control Stick Moved Down",13))
      255: 
       pst.str(String("Right Control Stick Moved Down",13))

    CASE PS2.get_RightX
      0:
        pst.str(String("Right Control Stick Moved Left",13))
      255:
        pst.str(String("Right Control Stick Moved Right",13))
     
    {Left Control Stick}
    if (PS2.get_LeftY <> 0) &(PS2.get_LeftY <> 255) & (PS2.get_LeftX <> 0) & (PS2.get_LeftX <> 255)
      pst.str(String("Left Control Stick Not Moving",13))

    CASE PS2.get_LeftY
      0:
        pst.str(String("Left Control Stick Moved Down",13))
      255:
       pst.str(String("Left Control Stick Moved Up",13))

    CASE PS2.get_LeftX
      0:
        pst.str(String("Left Control Stick Moved Left",13))
      255:
        pst.str(String("Left Control Stick Moved Right",13))



      


    {Angular Servo Test}
    waitcnt(clkfreq/4 + cnt)
    if (PS2.get_LeftY == 0)
      cogStop(1)
      coginit(1, MoveMotor(20,arm_servo+=10), @stack)
    else
      if (PS2.get_LeftY == 255)
        cogStop(1)
        coginit(1, MoveMotor(20,arm_servo-=10), @stack)    
    if (PS2.get_Data1 == $F7FF5A73)
      cogStop(2)
      coginit(2, MoveMotor(21,110), @stack+100)
    else
      if (PS2.get_Data1 == $FDFF5A73)
        cogStop(2)
        coginit(2, MoveMotor(21,80), @stack+100)    





      
    {Continous Servo Test}
    waitcnt(clkfreq/4 + cnt)
    if (PS2.get_LeftY <> 0) & (PS2.get_LeftY <> 255) & (PS2.get_LeftX <> 0) &  (PS2.get_LeftX <> 255)
      pst.str(String("Stop",13)) 
      cogStop(4)
      coginit(4, Drive(Stop), @stack)
    else
      if (PS2.get_LeftY == 255)
        pst.str(String("Reverse",13))
        cogStop(4)
        coginit(4, Drive(Reverse), @stack)    
      if (PS2.get_LeftY == 0)
        pst.str(String("Forward",13))
        cogStop(4)
        coginit(4, Drive(Forward), @stack)
      if (PS2.get_LeftX == 255)
        pst.str(String("Right",13))
        cogStop(4)
        coginit(4, Drive(Right), @stack)
      if (PS2.get_LeftX == 0)
        pst.str(String("Left",13))
        cogStop(4)
        coginit(4, Drive(Left), @stack+100)

          
PUB MoveMotor(Pin, position)                              
  dira[Pin]~~                                   
  repeat                                        
    outa[Pin]~~                                 
    waitcnt((clkfreq/100_000)*position+cnt)    
    outa[Pin]~                                 
    waitcnt(clkfreq/100+cnt)

PUB Drive(Direction)             ' Syntax: Drive( Direction to Drive in)  Options: Forward, Reverse, Right, Left, Stop
  if (direction == Forward)
      servo.Set(ServoCh1,2000)
      servo.Set(ServoCh2,2000)
      servo.Set(ServoCh3,1000)
      servo.Set(ServoCh4,1000)    
  if (direction == Reverse)
      servo.Set(ServoCh1,1000)
      servo.Set(ServoCh2,1000)
      servo.Set(ServoCh3,2000)
      servo.Set(ServoCh4,2000)  
  if (direction == Right)
      servo.Set(ServoCh1,2000)
      servo.Set(ServoCh2,2000)
      servo.Set(ServoCh3,2000)
      servo.Set(ServoCh4,2000)
  if (direction == Left)
      servo.Set(ServoCh1,1000)
      servo.Set(ServoCh2,1000)
      servo.Set(ServoCh3,1000)
      servo.Set(ServoCh4,1000)
  if (direction == Stop)
      servo.Set(ServoCh1,1500)
      servo.Set(ServoCh2,1500)
      servo.Set(ServoCh3,1500)
      servo.Set(ServoCh4,1500)
      